package com.hitqz.robot.driver.zmq.protocol.handler;

import com.example.tutorial.AddressBookProtos;
import com.hitqz.robot.driver.util.WheelCacheUtil;
import com.hitqz.robot.driver.util.WheelToolkit;
import lombok.extern.slf4j.Slf4j;
import org.springframework.stereotype.Component;

import java.util.HashMap;

/**
 * 设备当前速度处理类
 *
 * @author wengyx
 * @date 2021-09-14 10:54
 **/
@Slf4j
@Component
public class TwistStampedHandler implements HandlerZMQService {

    @Override
    public void handlerMsg(String robotSn, byte[] data) throws Exception {
        AddressBookProtos.TwistStamped twistStamped = AddressBookProtos.TwistStamped.parseFrom(data);
        String type = twistStamped.getDescriptorForType().getName();
        int objId = twistStamped.getHeader().getObj();
        int ackId = twistStamped.getHeader().getAck();
        //速度 主动上报
        if (objId==AddressBookProtos.TwistStamped.OBJ_ID.NUL_CAN_VEL_NTF_VALUE) {
            HashMap<String, Double> speedMap = new HashMap<>();
            speedMap.put("linear_x",twistStamped.getLinearX());
            speedMap.put("linear_y",twistStamped.getLinearY());
            speedMap.put("linear_z",twistStamped.getLinearZ());
            speedMap.put("angular_x",twistStamped.getAngularX());
            speedMap.put("angular_y",twistStamped.getAngularY());
            speedMap.put("angular_z",twistStamped.getAngularZ());
            WheelCacheUtil.put(WheelCacheUtil.KEY_SPEED, speedMap);
        }
        //释放锁
        WheelToolkit.release(ackId,type, objId, true);
    }

}
